mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-22 15:55:48 +03:00
Added throttleCorrection, batter, rcControls, gpsProfile, and gps config() macros
This commit is contained in:
parent
2f0f23ebfe
commit
00338cb854
9 changed files with 144 additions and 139 deletions
|
@ -693,7 +693,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
|
||||
sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
|
||||
sbufWriteU16(dst, rssi);
|
||||
if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
|
||||
if(batteryConfig()->multiwiiCurrentMeterOutput) {
|
||||
sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
|
||||
} else
|
||||
sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
|
||||
|
@ -785,24 +785,24 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
|
||||
|
||||
#ifdef GPS
|
||||
sbufWriteU8(dst, masterConfig.gpsConfig.provider); // gps_type
|
||||
sbufWriteU8(dst, gpsConfig()->provider); // gps_type
|
||||
sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
|
||||
sbufWriteU8(dst, masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
|
||||
sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas
|
||||
#else
|
||||
sbufWriteU8(dst, 0); // gps_type
|
||||
sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
|
||||
sbufWriteU8(dst, 0); // gps_ubx_sbas
|
||||
#endif
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
|
||||
sbufWriteU8(dst, batteryConfig()->multiwiiCurrentMeterOutput);
|
||||
sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
|
||||
sbufWriteU8(dst, 0);
|
||||
|
||||
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
|
||||
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatscale);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
|
||||
break;
|
||||
|
||||
case MSP_MOTOR_PINS:
|
||||
|
@ -872,17 +872,17 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_VOLTAGE_METER_CONFIG:
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatscale);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
|
||||
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
|
||||
break;
|
||||
|
||||
case MSP_CURRENT_METER_CONFIG:
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.currentMeterType);
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.batteryCapacity);
|
||||
sbufWriteU16(dst, batteryConfig()->currentMeterScale);
|
||||
sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
|
||||
sbufWriteU8(dst, batteryConfig()->currentMeterType);
|
||||
sbufWriteU16(dst, batteryConfig()->batteryCapacity);
|
||||
break;
|
||||
|
||||
case MSP_MIXER:
|
||||
|
@ -942,8 +942,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU16(dst, boardAlignment()->pitchDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->yawDegrees);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
|
||||
sbufWriteU16(dst, batteryConfig()->currentMeterScale);
|
||||
sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
|
||||
break;
|
||||
|
||||
case MSP_CF_SERIAL_CONFIG:
|
||||
|
@ -1068,9 +1068,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_RC_DEADBAND:
|
||||
sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband);
|
||||
sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband);
|
||||
sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband);
|
||||
sbufWriteU8(dst, rcControlsConfig()->deadband);
|
||||
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
|
||||
sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
|
||||
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
|
||||
break;
|
||||
|
||||
|
@ -1344,24 +1344,24 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
|
||||
|
||||
#ifdef GPS
|
||||
masterConfig.gpsConfig.provider = sbufReadU8(src); // gps_type
|
||||
gpsConfig()->provider = sbufReadU8(src); // gps_type
|
||||
sbufReadU8(src); // gps_baudrate
|
||||
masterConfig.gpsConfig.sbasMode = sbufReadU8(src); // gps_ubx_sbas
|
||||
gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
|
||||
#else
|
||||
sbufReadU8(src); // gps_type
|
||||
sbufReadU8(src); // gps_baudrate
|
||||
sbufReadU8(src); // gps_ubx_sbas
|
||||
#endif
|
||||
masterConfig.batteryConfig.multiwiiCurrentMeterOutput = sbufReadU8(src);
|
||||
batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
|
||||
masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
|
||||
compassConfig()->mag_declination = sbufReadU16(src) * 10;
|
||||
|
||||
masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
break;
|
||||
|
||||
case MSP_SET_MOTOR:
|
||||
|
@ -1417,9 +1417,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_RC_DEADBAND:
|
||||
masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
|
||||
masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
|
||||
masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
|
||||
rcControlsConfig()->deadband = sbufReadU8(src);
|
||||
rcControlsConfig()->yaw_deadband = sbufReadU8(src);
|
||||
rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
|
||||
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
|
@ -1649,17 +1649,17 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_VOLTAGE_METER_CONFIG:
|
||||
masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
break;
|
||||
|
||||
case MSP_SET_CURRENT_METER_CONFIG:
|
||||
masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
|
||||
masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
|
||||
masterConfig.batteryConfig.currentMeterType = sbufReadU8(src);
|
||||
masterConfig.batteryConfig.batteryCapacity = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterScale = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterOffset = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterType = sbufReadU8(src);
|
||||
batteryConfig()->batteryCapacity = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
|
@ -1735,8 +1735,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
|
||||
boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
|
||||
|
||||
masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
|
||||
masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterScale = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterOffset = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_CF_SERIAL_CONFIG:
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue